/*
* Copyright (C) 2011 Uni Osnabrück
*
* This file is part of the LSSR-Toolkit,
*
* LSSR is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* LSSR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307,USA

*/
/**
*	Pathplanner.cpp
*
* @date		13.01.2013
* @author Carlos Engelfried (cengelfr@uos.de)
* @author Clemens John (cjohn@uos.de)
*/

#include "PathPlanner.hpp"

namespace asteroids
{    
	PathPlanner::PathPlanner(std::string mapfile)	
	{
		std::ifstream map;
		string node_name,stream;
		std::stringstream ss;
		char buffer[1024];
		int numberOfNodes,i,node_in, node_out;
		float x_coord, y_coord, z_coord;
		//open (in)filestream
		map.open(mapfile.c_str());

		//Gewicht Eigenschaft der Kanten
		//EdgeWeightPropertyMap weight = get(boost::edge_weight_t(),m_graph);

		if(map.good())
		{
			//first line contains number of nodes in the graph
			map.getline(buffer,256);
			cout << "n = " << buffer << endl;
			numberOfNodes = atoi(buffer);
			//read nodes (name and coordinates)
			for(i=0;i<numberOfNodes;i++)
			{
					map.getline(buffer,256);
					cout << buffer << endl;
					ss << buffer << " ";
					ss >> node_name >> x_coord >> y_coord >> z_coord;
					//add node to graph, save coordinates and name
					addNode(x_coord,y_coord,z_coord,node_name);
										
			}
			//read edges and calculate weight
			while(map.good())
			{
				map.getline(buffer,256);
				ss << buffer << " ";
				ss >> node_in >> node_out;
				//add edge with weights to graph
				addEdge(node_in, node_out);
			}
		}
		else
		{
			//error message, if file does not exist
			cout << "File " << mapfile << " does not exist" << endl;
			exit(1);
		}

		map.close();


       //Print the graph to cout http://www.boost.org/doc/libs/1_36_0/libs/graph/doc/write-graphviz.html
       boost::write_graphviz(std::cout, m_graph);              

	}
	
	void PathPlanner::addNode(float x_coord, float y_coord, float z_coord, string name)
	{
		//push coordinates and name of nodes into vectors (at the end of the vector)
		m_navpoints.push_back(Vertex<float>(x_coord,y_coord,z_coord));
		m_node_names.push_back(name);

                //add node to graph
		Node node = boost::add_vertex(m_graph);

         //create map entry that assiciates the name of the node with the index of the node in the grah
         nodename2index.insert(make_pair(name, node));
	}

	
	
	void PathPlanner::addEdge(int node_in, int node_out)
	{
		
		//Full Kevin Bacon example:
        //http://www.boost.org/doc/libs/1_52_0/libs/graph/example/kevin-bacon.cpp
                
		Node in, out;
		
		//Nodes out of range are ignored
		//Check if node is inside range (size of nodelist). Nodes out of range are ignored
		if((unsigned)node_in < m_navpoints.size() && (unsigned)node_out < m_navpoints.size())
		{
			//create nodes
			in = boost::vertex(node_in,m_graph);
			out = boost::vertex(node_out,m_graph);

                        //add edge to graph
                        Edge edge = boost::add_edge(in, out, m_graph).first;

                        //calculate distance between nodes
                        float distance = m_navpoints[node_in].dist(m_navpoints[node_out]);

                        //create propert map. Documentation:
                        //http://www.boost.org/doc/libs/1_52_0/libs/property_map/doc/property_map.html
                        //http://www.boost.org/doc/libs/1_52_0/libs/graph/doc/using_property_maps.html
                        boost::property_map<Graph, boost::edge_weight_t>::type distance_map = get(boost::edge_weight, m_graph);
			
                        //add distance to property map
			boost::put(distance_map, edge, distance);
			
		}
		else
		{
			//error message
			cout << "Note: edge (" << node_in << "," << node_out << ") ignored, because out of range." << endl;
		}
	
			
	}

	std::list<Vertex<float> > PathPlanner::getPath(Vertex<float> position, std::string s, std::string e)
	{
            //create a list containig the shortest path
            std::list<Vertex<float> > path;
            
            //get the vertexdiscriptors of the given node names
            //http://www.cplusplus.com/reference/map/map/at/
            Node startnode;
            Node endnode;
            try {
                startnode = nodename2index.at(s);
            } catch (std::out_of_range& oor) {
                std::cerr << "Node " << s << " could not be found (out_of_range error in " << oor.what() << ")" << endl;
                return path;
            }
            try {
                endnode = nodename2index.at(e);                
            } catch (std::out_of_range& oor) {
                std::cerr << "Node " << e << " could not be found (out_of_range error in " << oor.what() << ")" << endl;
                return path;                
            }
            
            //create something unknown
            vector<Graph::vertex_descriptor> p(num_vertices(m_graph));
            vector<cost> d(num_vertices(m_graph));

            //do the astart search
            try {
                boost::astar_search(m_graph, startnode, DistanceHeuristic<Graph, cost>(m_navpoints, endnode),
                                    boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(AstarGoalVisitor<Node>(endnode)));
            } catch(found_goal fg) { //if the shortest path has been foud
                //put the vertices of the shortest path into the list
                for(Node v = endnode;; v = p[v]) {
                    path.push_front(m_navpoints[v]);
                    if(p[v] == v)
                        break;
                }
            }

            return path;
	}


	std::list<Vertex<float> > PathPlanner::getNavpoints()
	{	
		unsigned int i;
		//push navpoints into list
		std::list<Vertex<float> > ret;
		for(i = 0;i<m_navpoints.size();i++) ret.push_back(m_navpoints[i]);
		return ret;
	}
	
}
